{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridGaussianISAM"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridGaussianISAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridGaussianISAM` implements the Incremental Smoothing and Mapping (ISAM) algorithm for **hybrid** factor graphs, specifically `HybridGaussianFactorGraph`s. It inherits from `gtsam.ISAM<HybridBayesTree>`, meaning it maintains an underlying `HybridBayesTree` representing the smoothed posterior distribution $P(X, M | Z)$ over continuous variables $X$, discrete variables $M$, given measurements $Z$.\n",
        "\n",
        "The key feature is the `update` method, which efficiently incorporates new factors (measurements) into the existing `HybridBayesTree` without re-processing the entire history. This involves:\n",
        "1. Identifying the portion of the Bayes tree affected by the new factors.\n",
        "2. Removing the affected cliques (orphans).\n",
        "3. Re-eliminating the variables in the orphaned cliques along with the new factors.\n",
        "4. Merging the newly created Bayes sub-tree back into the main tree.\n",
        "\n",
        "It provides an incremental solution for problems involving both continuous and discrete variables, where the underlying system dynamics are linear or have been linearized (resulting in a `HybridGaussianFactorGraph`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "from gtsam import (\n",
        "    HybridGaussianISAM, HybridGaussianFactorGraph, HybridBayesTree,\n",
        "    JacobianFactor, DecisionTreeFactor, HybridGaussianFactor,\n",
        "    DiscreteValues, VectorValues, HybridValues, Ordering\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization\n",
        "\n",
        "Can be initialized empty or from an existing `HybridBayesTree`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Empty HybridGaussianISAM created.\n",
            "\n",
            "HybridGaussianISAM from initial HybridBayesTree:\n",
            "HybridBayesTree\n",
            ": cliques: 2, variables: 2\n",
            "HybridBayesTree\n",
            "-p(x0)\n",
            "  R = [ 1 ]\n",
            "  d = [ 0 ]\n",
            "  mean: 1 elements\n",
            "  x0: 0\n",
            "  logNormalizationConstant: -0.918939\n",
            "  No noise model\n",
            "HybridBayesTree\n",
            "- P( d0 ):\n",
            " f[ (d0,2), ]\n",
            "(d0, 0) | 0.6        | 0\n",
            "(d0, 1) | 0.4        | 1\n",
            "number of nnzs: 2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# 1. Empty ISAM\n",
        "hisam1 = gtsam.HybridGaussianISAM()\n",
        "print(\"Empty HybridGaussianISAM created.\")\n",
        "\n",
        "# 2. From existing HybridBayesTree\n",
        "# Create a minimal initial graph and Bayes tree P(D0), P(X0)\n",
        "initial_graph = gtsam.HybridGaussianFactorGraph()\n",
        "dk0 = (D(0), 2)\n",
        "initial_graph.add(DecisionTreeFactor([dk0], \"0.6 0.4\")) # P(D0)\n",
        "initial_graph.add(JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))) # P(X0)\n",
        "ordering = gtsam.Ordering([X(0), D(0)])\n",
        "initial_hbt = initial_graph.eliminateMultifrontal(ordering)\n",
        "\n",
        "hisam2 = gtsam.HybridGaussianISAM(initial_hbt)\n",
        "print(\"\\nHybridGaussianISAM from initial HybridBayesTree:\")\n",
        "hisam2.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Incremental Updates\n",
        "\n",
        "The `update` method takes a `HybridGaussianFactorGraph` containing new factors to be added."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Adding Update 1 Factors:\n",
            "\n",
            "size: 2\n",
            "Factor 0\n",
            "GaussianFactor:\n",
            "\n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.316228\n",
            "\n",
            "Factor 1\n",
            "HybridGaussianFactor:\n",
            "Hybrid [x1; d0]{\n",
            " Choice(d0) \n",
            " 0 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.5\n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 5 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "\n",
            "\n",
            "ISAM state after Update 1:\n",
            "HybridBayesTree\n",
            ": cliques: 3, variables: 3\n",
            "HybridBayesTree\n",
            "- P( d0 ):\n",
            " f[ (d0,2), ]\n",
            "(d0, 0) | 0.976859   | 0\n",
            "(d0, 1) | 0.0231405  | 1\n",
            "number of nnzs: 2\n",
            "\n",
            "HybridBayesTree\n",
            "| - P( x1 | d0)\n",
            " Discrete Keys = (d0, 2), \n",
            " logNormalizationConstant: -0.123394\n",
            "\n",
            " Choice(d0) \n",
            " 0 Leaf p(x1)\n",
            "  R = [ 2.21565 ]\n",
            "  d = [ 2.21565 ]\n",
            "  mean: 1 elements\n",
            "  x1: 1\n",
            "  logNormalizationConstant: -0.123394\n",
            "  No noise model\n",
            "\n",
            " 1 Leaf p(x1)\n",
            "  R = [ 1.3817 ]\n",
            "  d = [ 4.27669 ]\n",
            "  mean: 1 elements\n",
            "  x1: 3.09524\n",
            "  logNormalizationConstant: -0.595625\n",
            "  No noise model\n",
            "\n",
            "HybridBayesTree\n",
            "| | -p(x0 | x1)\n",
            "  R = [ 3.31662 ]\n",
            "  S[x1] = [ -3.01511 ]\n",
            "  d = [ -3.01511 ]\n",
            "  logNormalizationConstant: 0.280009\n",
            "  No noise model\n",
            "\n",
            "Adding Update 2 Factors:\n",
            "\n",
            "size: 1\n",
            "Factor 0\n",
            "GaussianFactor:\n",
            "\n",
            "  A[x1] = [\n",
            "\t-1\n",
            "]\n",
            "  A[x2] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 2 ]\n",
            "  Noise model: unit (1) \n",
            "\n",
            "\n",
            "ISAM state after Update 2:\n",
            "HybridBayesTree\n",
            ": cliques: 3, variables: 4\n",
            "HybridBayesTree\n",
            "- P( d0 ):\n",
            " f[ (d0,2), ]\n",
            "(d0, 0) | 0.976859   | 0\n",
            "(d0, 1) | 0.0231405  | 1\n",
            "number of nnzs: 2\n",
            "\n",
            "HybridBayesTree\n",
            "| - P( x1 x2 | d0)\n",
            " Discrete Keys = (d0, 2), \n",
            " logNormalizationConstant: -1.04233\n",
            "\n",
            " Choice(d0) \n",
            " 0 Leaf p(x1 x2 )\n",
            "  R = [   2.43086 -0.411377 ]\n",
            "      [         0  0.911465 ]\n",
            "  d = [ 1.19673  2.7344 ]\n",
            "  mean: 2 elements\n",
            "  x1: 1\n",
            "  x2: 3\n",
            "  logNormalizationConstant: -1.04233\n",
            "  No noise model\n",
            "\n",
            " 1 Leaf p(x1 x2 )\n",
            "  R = [   1.70561 -0.586302 ]\n",
            "      [         0  0.810093 ]\n",
            "  d = [ 2.29191 4.12761 ]\n",
            "  mean: 2 elements\n",
            "  x1: 3.09524\n",
            "  x2: 5.09524\n",
            "  logNormalizationConstant: -1.51456\n",
            "  No noise model\n",
            "\n",
            "HybridBayesTree\n",
            "| | -p(x0 | x1)\n",
            "  R = [ 3.31662 ]\n",
            "  S[x1] = [ -3.01511 ]\n",
            "  d = [ -3.01511 ]\n",
            "  logNormalizationConstant: 0.280009\n",
            "  No noise model\n"
          ]
        }
      ],
      "source": [
        "# Start with hisam2 from above\n",
        "hisam = hisam2\n",
        "\n",
        "# --- Update 1: Add factors connecting X0, X1, D0 ---\n",
        "update1_graph = gtsam.HybridGaussianFactorGraph()\n",
        "# Add P(X1 | X0) = N(X0+1, 0.1)\n",
        "update1_graph.add(JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1))))\n",
        "# Add P(X1 | D0) = mixture N(1, 0.25); N(5, 1.0)\n",
        "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n",
        "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n",
        "update1_graph.add(HybridGaussianFactor(dk0, [gf0, gf1]))\n",
        "\n",
        "print(\"\\nAdding Update 1 Factors:\")\n",
        "update1_graph.print()\n",
        "\n",
        "hisam.update(update1_graph)\n",
        "print(\"\\nISAM state after Update 1:\")\n",
        "hisam.print()\n",
        "\n",
        "# --- Update 2: Add factor connecting X1, X2 ---\n",
        "update2_graph = gtsam.HybridGaussianFactorGraph()\n",
        "update2_graph.add(JacobianFactor(X(1), -np.eye(1), X(2), np.eye(1), np.array([2.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))\n",
        "\n",
        "print(\"\\nAdding Update 2 Factors:\")\n",
        "update2_graph.print()\n",
        "\n",
        "hisam.update(update2_graph)\n",
        "print(\"\\nISAM state after Update 2:\")\n",
        "hisam.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Solution and Marginals\n",
        "\n",
        "After updates, the underlying `HybridBayesTree` can be used to obtain the current MAP estimate or calculate marginals, similar to the batch case."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Current MAP Solution from ISAM:\n",
            "HybridValues: \n",
            "  Continuous: 3 elements\n",
            "  x0: 2.67796e-16\n",
            "  x1: 1\n",
            "  x2: 3\n",
            "  Discrete: (d0, 0)\n",
            "  Nonlinear\n",
            "Values with 0 values:\n"
          ]
        }
      ],
      "source": [
        "# Get the current MAP estimate from the ISAM object\n",
        "# ISAM inherits optimize() from HybridBayesTree\n",
        "current_map_solution = hisam.optimize()\n",
        "print(\"\\nCurrent MAP Solution from ISAM:\")\n",
        "current_map_solution.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "9cc5afed",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "MPE Assignment: DiscreteValues{7205759403792793600: 0}\n",
            "\n",
            "GaussianBayesTree for MPE assignment:\n",
            ": cliques: 3, variables: 3\n",
            "- p()\n",
            "  R = Empty (0x0)\n",
            "  d = Empty (0x1)\n",
            "  mean: 0 elements\n",
            "  logNormalizationConstant: -0\n",
            "  No noise model\n",
            "| - p(x1 x2 )\n",
            "  R = [   2.43086 -0.411377 ]\n",
            "      [         0  0.911465 ]\n",
            "  d = [ 1.19673  2.7344 ]\n",
            "  mean: 2 elements\n",
            "  x1: 1\n",
            "  x2: 3\n",
            "  logNormalizationConstant: -1.04233\n",
            "  No noise model\n",
            "| | - p(x0 | x1)\n",
            "  R = [ 3.31662 ]\n",
            "  S[x1] = [ -3.01511 ]\n",
            "  d = [ -3.01511 ]\n",
            "  logNormalizationConstant: 0.280009\n",
            "  No noise model\n"
          ]
        }
      ],
      "source": [
        "# Access the underlying HybridBayesTree methods\n",
        "# Get a specific GaussianBayesTree for an MPE assignment\n",
        "mpe = hisam.mpe()\n",
        "print(\"\\nMPE Assignment:\", mpe)\n",
        "gbt_mpe = hisam.choose(mpe)\n",
        "print(\"\\nGaussianBayesTree for MPE assignment:\")\n",
        "gbt_mpe.print()"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
